/*
 * ROSIF.cpp
 *
 *  Created on: Nov 17, 2012
 *      Author: bouchier
 */

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include "qetime.h"
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/MultiArrayLayout.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int32MultiArray.h>
#include <CQEI2C.h>
#include <ControlledMotor.h>
#include <JRoverMotors.h>
#include <JRoverSensors.h>
#include "ROSIF.h"

#define METERS_TO_INCHES 39.37

extern JRoverMotors jRoverMotors;
extern JRoverSensors jRoverSensors;
extern CQEI2C i2c;

// local variables to hold the last values of the twist message received from ROS subscription
static float twistLinear;
static float twistAngular;
static bool twistMsgReceived;

// ROS declarations & definitions
geometry_msgs::Twist twist;
ros::NodeHandle  nh;

ROSIF::ROSIF() {
	telemetryTestFlag = false;
	rosEnabled = false;
	twistMsgReceived = false;
	rosLinear = 0.0;
	rosAngular = 0.0;
}

ROSIF::~ROSIF() {
}

//! Callback when Velocity msg is received
void driveRoverMsgCb(const geometry_msgs::Twist& twist_msg){
	twistLinear = twist_msg.linear.x;
	twistAngular = twist_msg.angular.z;
	twistMsgReceived = true;
}
ros::Subscriber<geometry_msgs::Twist> driveRoverSub("cmd_vel", driveRoverMsgCb );

// declarations for published multi-arrays
// Robot Telemetry message (overall requested & actual velocity & rotational velocity
std_msgs::Float32MultiArray robotTelemetryMsg;
std_msgs::MultiArrayDimension robotTelemetryDim;
ros::Publisher robotTelemetry("robotTelemetry", &robotTelemetryMsg);
char robotMotionLabel[] = "robotTelemetry";

// Drive motor message (velocity, power, distance, requested velocity) for each wheel
std_msgs::Float32MultiArray driveMotorMsg;
std_msgs::MultiArrayDimension driveMotorDim[2];
ros::Publisher driveMotorPub ("driveMotorTelemetry", &driveMotorMsg);
char driveMotorNumLabel[] = "driveMotorNum";
char driveMotorValueLabel[] = "driveMotorValues";

// Steering motor message (rqAngle, angle, power) for each wheel
std_msgs::Int32MultiArray steerMotorMsg;
std_msgs::MultiArrayDimension steerMotorDim[2];
ros::Publisher steerMotorPub ("steerMotorTelemetry", &steerMotorMsg);
char steerMotorNumLabel[] = "steerMotorNum";
char steerMotorValueLabel[] = "steerMotorValues";

//! Initialize rosserial
/*
 * Check for /opt/usr/bin/RosSrvrIp and read an IP address from it
 * \return True if rosserial initialized successfully
 */
bool ROSIF::initRos()
{
	int fd, rv;
	int robotTelemetrySize;		// number of floats published in robotTelemetry message

	// check for the file containing the rosserial server IP
	fd = open("/opt/usr/bin/RosSrvrIp", O_RDONLY);
	if (fd < 0) {
		printf("ROS NOT INITIALIZED: /opt/usr/bin/RosSrvrIp not found\n");
		return false;
	}
	rv = read(fd, rosSrvrIp, 19);	// read up to 19 bytes
	if (rv < 1) {
		perror("ERROR reading from RosSrvrIp: ");
		return false;
	}

	rosSrvrIp[rv-1] = '\0'; 						// null terminate the rossrvrip string

	// initialize ROS
	nh.initNode(rosSrvrIp);
	nh.subscribe(driveRoverSub);

	// set up robot motion multiArray
	robotTelemetrySize = jRoverSensors.getRobotTelemetrySize();
	robotTelemetryMsg.layout.dim = &robotTelemetryDim;
	robotTelemetryMsg.layout.dim[0].label = robotMotionLabel;
	robotTelemetryMsg.layout.dim[0].size = robotTelemetrySize;
	robotTelemetryMsg.layout.dim[0].stride = 1 * robotTelemetrySize;
	robotTelemetryMsg.layout.dim_length = 1;
	robotTelemetryMsg.data_length = robotTelemetrySize;
	robotTelemetryMsg.layout.data_offset = 0;
	robotTelemetryMsg.data = &(jRoverSensors.robotTelemetry[0]);
	nh.advertise(robotTelemetry);

	// set up drive motor telemetry multiArray
	driveMotorMsg.layout.dim = driveMotorDim;
	driveMotorMsg.layout.dim[0].label = driveMotorNumLabel;
	driveMotorMsg.layout.dim[0].size = 6;						// six 4-byte values - not used
	driveMotorMsg.layout.dim[0].stride = 6 * 4 * 4;				// not used
	driveMotorMsg.layout.dim[1].label = driveMotorValueLabel;
	driveMotorMsg.layout.dim[1].size = 4 * 4;					// four 4-byte values
	driveMotorMsg.layout.dim[1].stride = 4;
	driveMotorMsg.layout.dim_length = 2;
	driveMotorMsg.data_length = 24;
	driveMotorMsg.layout.data_offset = 0;
	driveMotorMsg.data = &(jRoverMotors.driveMotorTelemetry[0][0]);
	nh.advertise(driveMotorPub);

	// set up steer motor telemetry multiArray
	steerMotorMsg.layout.dim = steerMotorDim;
	steerMotorMsg.layout.dim[0].label = steerMotorNumLabel;
	steerMotorMsg.layout.dim[0].size = 6;						// six 4-byte values - not used
	steerMotorMsg.layout.dim[0].stride = 6 * 3 * 4;				// not used
	steerMotorMsg.layout.dim[1].label = steerMotorValueLabel;
	steerMotorMsg.layout.dim[1].size = 3 * 4;					// three 4-byte values
	steerMotorMsg.layout.dim[1].stride = 4;
	steerMotorMsg.layout.dim_length = 2;
	steerMotorMsg.data_length = 18;
	steerMotorMsg.layout.data_offset = 0;
	steerMotorMsg.data = &(jRoverMotors.steerMotorTelemetry[0][0]);
	nh.advertise(steerMotorPub);

	rosEnabled = true;
	printf("rosserial initialized using rosserial server IP %s\n", rosSrvrIp);

	return true;
}

//! Check for new messages to send or recieve from rosserial_python proxy
/*!
 * If rosEnabled is true, send or receive any pending messages. Check if a twist message was received
 * and copy to object if so.
 */
void ROSIF::poll()
{
	if (rosEnabled) {
		nh.spinOnce();
	} else {
		printf("WARNING: ROSIF::spinOnce() called but rosEnabled is false\n");
	}

	// check if we got a twist message, and copy to object if yes
	if (twistMsgReceived) {
		twistMsgReceived = false;
		rosLinear = twistLinear * METERS_TO_INCHES;
		rosAngular = twistAngular;
		printf("got Twist msg from ROS, linear: %0.1f, angular: %0.1f\n", rosLinear, rosAngular);
	}
}

//! publish drive motor telemetry. Fill it with test data if telemetryTestFlag is true
void ROSIF::publishDriveMotorTelemetry()
{
	jRoverMotors.getDriveMotorsTelemetry();
	if (telemetryTestFlag)
		jRoverMotors.setTestDriveMotorTelemetry(1.0);		// overwrite the data if in test mode

	// now publish the data to ROS
	driveMotorPub.publish(&driveMotorMsg);
}

//! publish steer motor telemetry. Fill it with test data if telemetryTestFlag is true
void ROSIF::publishSteerMotorTelemetry()
{
	if (telemetryTestFlag)
		jRoverMotors.setTestSteerMotorTelemetry(1.0);		// overwrite the data if in test mode

// now publish the data to ROS
	steerMotorPub.publish(&steerMotorMsg);
}

//! Publish robot telemetry.
/*!
 * See the definition of robotTelemetry variable for its contents. Note that this module
 * plugs in ros speed request into the telemetry message.
 * FIXME: this is a hack
 */
void ROSIF::publishRobotTelemetry()
{
	jRoverSensors.robotTelemetry[0] = rosLinear;
	jRoverSensors.robotTelemetry[1] = rosAngular;
	robotTelemetry.publish(&robotTelemetryMsg);
}

//! Fill the robotTelemetry message with test data
void ROSIF::setTestRobotMotionData(float f)
{
	robotTelemetryMsg.data[0] = 1.0 + f;
	robotTelemetryMsg.data[1] = 2.0 + f;
	robotTelemetryMsg.data[2] = -3.1 - f;
	robotTelemetryMsg.data[3] = 4.2 - f;
}

//! Get the last requested linear & angular speeds
/*!
 * \param linearRef Reference to a float where the requested linear speed in ips will be placed
 * \param angularRef Reference to a float where the requested angular speed in rad/s will be placed
 */
void ROSIF::getRosLinearAngular(float& linearRef, float& angularRef)
{
	linearRef = rosLinear;
	angularRef = rosAngular;
}
